/* -*- Mode: C; indent-tabs-mode: t; c-basic-offset: 4; tab-width: 2 -*- */
/**
* @file TestDoubleEngine.c
* 
* Library for testing the doublengine library code.
* 
* @author Miguel Ángel Vílchez López 2010 <sacra@openmatica.es>
*
* Codigo is free software: you can redistribute it and/or modify it
* under the terms of the GNU General Public License as published by the
* Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
* 
* Codigo is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
* See the GNU General Public License for more details.
* 
* You should have received a copy of the GNU General Public License along
* with this program.  If not, see <http://www.gnu.org/licenses/>.
*/

#include <stdlib.h>
#include <string.h>
#include <doubleengine.cpp>
#include <timer.cpp>

/**
* @example TestDoubleEngine.c
*/

int main(int argc, char *argv[])
{
	if(argc<2)
	{
		puts("\nUSE:\nTestDoubleEngine <type>\n");
		puts("types:");
		puts("\tmoveForward <acceleration> <useconds timer> <max_value> ");
		puts("\tmoveBackward <acceleration> <useconds timer> <min_value>");
		puts("\tmoveForwardRight <acceleration> <useconds timer> <min_value> <max_value> <diff>");
		puts("\tmoveForwardLeft <acceleration> <useconds timer> <min_value> <max_value> <diff>");
		puts("\tmoveBackwardRight <acceleration> <useconds timer> <min_value> <max_value> <diff>");
		puts("\tmoveBackwardLeft <acceleration> <useconds timer> <min_value> <max_value> <diff>");
		puts("\tturnLeft <acceleration> <useconds timer> <min_value> <max_value> <diff>");
		puts("\tturnRight <acceleration> <useconds timer> <min_value> <max_value> <diff>");
		puts("\t  e.g.");
		puts("\t    TestDoubleEngine moveForward 6 100000 50 => Move forward until 50, steps of 6 each 0,1 second.");
		puts("\t    TestDoubleEngine moveForwardRight 6 100000 -100 100 5 => Move forward right until 100,, difference of 5, steps of 6 each 0,1 second.");
		puts("\t    TestDoubleEngine turnLeft 6 100000 -50 100 20 => Turn left, difference of 20, steps of 6 each 0,1 second.\n");

		exit(EXIT_SUCCESS);
	}

	CMTimer temp(atol(argv[3]));

	//Forward
	if (strcmp(argv[1],"moveForward") == 0)
	{
		CMDoubleEngine engine(atol(argv[2]),0,atoi(argv[4]));
		engine.go(32767);

		printf("Moving Forward\n");
		while(engine.getLeftEngine() < atoi(argv[4]))
		{
			if(temp.check()==1)
			{
				engine.update();
				printf("Values\tleft:%d\tright:%d\n",engine.getLeftEngine(),engine.getRightEngine());
			}
		}
	}
	
	//Backward
	if (strcmp(argv[1],"moveBackward") == 0)
	{
		CMDoubleEngine engine(atol(argv[2]),atoi(argv[4]),0);
		engine.go(-32768);
		printf("Moving Backward\n");
		while(engine.getLeftEngine() > atoi(argv[4]))
		{
			if(temp.check()==1)
			{
				engine.update();
				printf("Values\tleft:%d\tright:%d\n",engine.getLeftEngine(),engine.getRightEngine());
			}
		}
	}
	
	//ForwardRight
	if (strcmp(argv[1],"moveForwardRight") == 0)
	{
		CMDoubleEngine engine(atol(argv[2]),atoi(argv[4]),atoi(argv[5]));
		engine.go(32768);
		printf("Moving Forward Right\n");
		while(1)
		{
			if(temp.check()==1)
			{
				engine.goForwardRight(atoi(argv[6]));
				engine.update();
				printf("Values\tleft:%d\tright:%d\n",engine.getLeftEngine(),engine.getRightEngine());
			}
		}
	}
	
	//ForwardLeft
	if (strcmp(argv[1],"moveForwardLeft") == 0)
	{
		CMDoubleEngine engine(atol(argv[2]),atoi(argv[4]),atoi(argv[5]));
		engine.go(32768);
		printf("Moving Forward Left\n");
		while(1)
		{
			if(temp.check()==1)
			{
				engine.goForwardLeft(atoi(argv[6]));
				engine.update();
				printf("Values\tleft:%d\tright:%d\n",engine.getLeftEngine(),engine.getRightEngine());
			}
		}
	}

	//BackwardRight
	if (strcmp(argv[1],"moveBackwardRight") == 0)
	{
		CMDoubleEngine engine(atol(argv[2]),atoi(argv[4]),atoi(argv[5]));
		engine.go(-32768);
		printf("Moving Backward Right\n");
		while(1)
		{
			if(temp.check()==1)
			{
				engine.goBackwardRight(atoi(argv[6]));
				engine.update();
				printf("Values\tleft:%d\tright:%d\n",engine.getLeftEngine(),engine.getRightEngine());
			}
		}
	}
	
	//ForwardLeft
	if (strcmp(argv[1],"moveBackwardLeft") == 0)
	{
		CMDoubleEngine engine(atol(argv[2]),atoi(argv[4]),atoi(argv[5]));
		engine.go(-32768);
		printf("Moving Backward Left\n");
		while(1)
		{
			if(temp.check()==1)
			{
				engine.goBackwardLeft(atoi(argv[6]));
				engine.update();
				printf("Values\tleft:%d\tright:%d\n",engine.getLeftEngine(),engine.getRightEngine());
			}
		}
	}
	
	//TurnRight
	if (strcmp(argv[1],"turnRight") == 0)
	{
		CMDoubleEngine engine(atol(argv[2]),atoi(argv[4]),atoi(argv[5]));
		engine.go(32768);
		printf("Turning Right\n");
		while(1)
		{
			if(temp.check()==1)
			{
				engine.turnRight(atoi(argv[6]));
				engine.update();
				printf("Values\tleft:%d\tright:%d\n",engine.getLeftEngine(),engine.getRightEngine());
			}
		}
	}
	
	//TurnLeft
	if (strcmp(argv[1],"turnLeft") == 0)
	{
		CMDoubleEngine engine(atol(argv[2]),atoi(argv[4]),atoi(argv[5]));
		engine.go(32768);
		printf("Turning Left\n");
		while(1)
		{
			if(temp.check()==1)
			{
				engine.turnLeft(atoi(argv[6]));
				engine.update();
				printf("Values\tleft:%d\tright:%d\n",engine.getLeftEngine(),engine.getRightEngine());
			}
		}
	}

}
